import Controller from "node-pid-controller";
interface Point {
    x: number;
    y: number;
}
interface InputTemplate<T> {
    target: T;
    tracker: T;
    usePID?: boolean;
    ctrConfig?: controllerConfig;
    space?: number;
    getPosition?: (point: T) => Point;
}
interface controllerConfig {
    k_p?: number;
    k_i?: number;
    k_d?: number;
    dt?: number;
    target?: number;
}

export class Tracking<T = Point> {
    controller?: Controller;
    target: T;
    tracker: T;
    usePID: boolean;
    /**
     * 用于获取输入对象的坐标，可以覆盖
     * @param input 输入的对象
     * @return \{x,y} 返回坐标
     */
    public getPosition({ x, y }: any): Point {
        return { x, y };
    }
    /** 视线追踪算法，用于跟随的更新 */
    constructor({
        target,
        tracker,
        usePID = true,
        getPosition,
        ctrConfig = {},
        space = 0,
    }: InputTemplate<T>) {
        this.target = target;
        this.tracker = tracker;
        this.usePID = usePID;
        if (usePID) {
            this.controller = this.createControl(ctrConfig);
            this.controller.setTarget(space);
        }
        if (typeof getPosition === "function") this.getPosition = getPosition;
    }

    /** 创建 PID 控制器用于产生控制系数，实现一个动态的变化效果 */
    private createControl({
        k_p = 0.5,
        k_i = 0.001,
        k_d = 0.01,
        dt = 0.1,
    }: controllerConfig) {
        return new Controller({
            k_p,
            k_i,
            k_d,
            dt,
        });
    }

    /**计算对象与目标之间的距离关系 */
    private calc(point: T, target: T) {
        const { x: px, y: py } = this.getPosition(point);
        const { x: tx, y: ty } = this.getPosition(target);
        var vx = px - tx,
            vy = py - ty,
            len = Math.sqrt(vx * vx + vy * vy),
            dx = -vx / len,
            dy = -vy / len;
        return { x: dx, y: dy, len };
    }
    /**
     * 更新跟随物体的速度
     * @param userParams  默认 0.1 用户提供的加成数值
     * @returns \{x,y} x,y 方向上的速度
     */
    public update(userParams = 0.1) {
        const { x, y, len } = this.calc(this.target, this.tracker);

        // 由 pid 算法提供的纠正系数
        const correction = this.usePID ? this.controller!.update(len) : -10;

        return {
            x: x * correction * userParams,
            y: y * correction * userParams,
        };
    }
}
